#include "pid.h"

#include "stm32f1xx_hal.h"
#include "tim.h"

int Pid_Init(Pid_HandleTypeDef *handle, uint32_t ti, float kp, float ki,
             float kd, float max_intg, float max_out, float target,
             float alpha) {
  if (handle == NULL) return -1;
  handle->ti = ti;
  handle->kp = kp;
  handle->ki = ki * ti;
  handle->kd = kd / ti;
  handle->max_intg = max_intg;
  handle->max_out = max_out;
  handle->target = target;
  handle->alpha = alpha;

  handle->intg = 0;
  handle->last_error = 0;
  handle->last_real = 0;
  handle->out = 0;
  return 0;
}

void Pid_SetTi(Pid_HandleTypeDef *handle, uint32_t ti) {
  if (handle == NULL) return;
  double ratio = (double)ti / (double)handle->ti;
  handle->ki *= ratio;
  handle->kd /= ratio;
  handle->ti = ti;
}

void Pid_SetK(Pid_HandleTypeDef *handle, float kp, float ki, float kd) {
  if (handle == NULL) return;
  handle->kp = kp;
  handle->ki = ki * handle->ti;
  handle->kd = kd / handle->ti;
}
void Pid_SetMax(Pid_HandleTypeDef *handle, float max_intg, float max_out) {
  if (handle == NULL) return;
  handle->max_intg = max_intg;
  handle->max_out = max_out;
}
void Pid_SetTarget(Pid_HandleTypeDef *handle, float target) {
  if (handle == NULL) return;
  handle->target = target;
}
void Pid_SetAlpha(Pid_HandleTypeDef *handle, float alpha) {
  if (handle == NULL) return;
  handle->alpha = alpha;
}

// version 1
float Pid_Calc1(Pid_HandleTypeDef *handle, float reality) {
  // 临时变量放在这里
  // 由于可能需要使用中断，则出于可重入性，这些变量都要放在Handler中或把static去掉
  // 计算误差
  float error = handle->target - reality;
  // 计算微分项
  float d_out = handle->kd * (error - handle->last_error);
  // 计算比例项
  float p_out = handle->kp * error;
  // 计算积分项
  handle->intg += error;
  // 积分限幅
  if (handle->intg > handle->max_intg) handle->intg = handle->max_intg;
  if (handle->intg < -handle->max_intg) handle->intg = -handle->max_intg;
  // 计算输出
  float out = p_out + d_out + handle->intg * handle->ki;
  // 输出限幅
  if (out > handle->max_out) out = handle->max_out;
  if (out < -handle->max_out) out = -handle->max_out;
  // 保存误差
  handle->last_error = error;

  return out;
}

// version 2
// 改进输出限额，添加输出滤波，改进修改ki时系统的表现
float Pid_Calc(Pid_HandleTypeDef *handle, float reality) {
  float error = handle->target - reality;
  float d_err = handle->kd * (error - handle->last_error);
  float p_err = handle->kp * error;
  handle->intg += handle->ki * error;
  handle->last_error = error;
  if (handle->intg > handle->max_intg) handle->intg = handle->max_intg;
  if (handle->intg < -handle->max_intg) handle->intg = -handle->max_intg;
  float out = p_err + d_err + handle->intg;
  if (out > handle->max_out) {
    handle->intg -= out - handle->max_out;
    out = handle->max_out;
  } else if (out < -handle->max_out) {
    handle->intg -= out + handle->max_out;
    out = -handle->max_out;
  }
  handle->out = handle->out * handle->alpha + out * (1 - handle->alpha);
  return out;
}

// version 3
// 使用PonM
// 烂尾了，不写了。。。
float Pid_Calc3(Pid_HandleTypeDef *handle, float reality) { return 0; }
